﻿using System;
using System.Collections.Generic;
using System.Text;
using JoystickInterface;
using System.Reflection;

namespace JoypadParser
{
    public class JoyPacket
    {

        private const char ASCIIHEADER = 'S';
        private const char ASCIIFOOTER = 'E';
        private const string LENPLACEHOLDER = "LEN";
        private const string ERRORPLACEHOLDER = "ERROR";
        private int _axisA;
        private int _axisB;
        private int _axisC;
        private int _axisD;
        private int _buttons;
        private int _time;
        //private int _error;

        private const int TYPE = 0x01;


        public JoyPacket(Joystick joystick)
        {
            PropertyInfo[] settings = joystick.Settings;
            bool[] inv = joystick.InvertedAxis;


            //tramite reflection recupero i valori degli assi scelti nelle impostazioni
            _axisA = Convert.ToInt32 (settings[0].GetValue(joystick,null));
            _axisB = Convert.ToInt32(settings[1].GetValue(joystick, null));
            _axisC = Convert.ToInt32(settings[2].GetValue(joystick, null));
            _axisD = Convert.ToInt32(settings[3].GetValue(joystick, null));

            //Limitiamo il joypad a 10 bit (1024 valori)
            _axisA = _axisA >> 6;
            _axisB = _axisB >> 6;
            _axisC = _axisC >> 6;
            _axisD = _axisD >> 6;

            _axisA = ManageAxis(_axisA);
            _axisB = ManageAxis(_axisB);
            _axisC = ManageAxis(_axisC);
            _axisD = ManageAxis(_axisD);

            //inverto i valori togliendo 1024 e cambiando di segno
            if (inv[0]) // asse A invertito
            {
                _axisA = -(_axisA - 1024);
            }

            if (inv[1])  // asse B invertito
            {
                _axisB = -(_axisB - 1024);
            }

            if (inv[2])  // asse C invertito
            {
                _axisC = -(_axisC - 1024);
            }

            if (inv[3])  // asse D invertito
            {
                _axisD = -(_axisD - 1024);
            }


            _buttons = CalcolaInteroButtons(joystick.Buttons);
            //timestamp...intero compreso tra 0 e 65535, ci sono 10000 ticks in un millisecondo
            long millisecondi = DateTime.Now.Ticks / 1000; // 1E-4s
            _time = Convert.ToInt32(millisecondi % 60000);
            //codice di errore
            //_error = _axisA + _axisB + _axisC + _axisD + _buttons;
        }

        private int ManageAxis(int input)
        {
            int deadzone = 40;
            int negative = 1;

            int output = 0;
            //manage dead zone and exp curve
            int axis = input - 512;
            if (axis < 0) negative = -1;
            axis = Math.Abs(axis);
            if (axis < deadzone)
            {
                output = 0;
            }
            else
            {
                axis = axis - deadzone;
                double val = Math.Exp((double)axis / (512-deadzone))*2;
                double scale = (Math.E - 1)*2;
                output = Convert.ToInt32((val-1*2)/scale * 512);
            }
            return output * negative + 512;
        }
        public JoyPacket(int a, int b, int c, int d, int buttons)
        {
            //Limitiamo il joypad a 10 bit (1024 valori)
            _axisA = a;
            _axisB = b;
            _axisC = c;
            _axisD = d;
            _buttons = buttons;
            //timestamp...intero compreso tra 0 e 65535, ci sono 10000 ticks in un millisecondo
            long millisecondi = DateTime.Now.TimeOfDay.Ticks / 1000;
            _time = Convert.ToInt32(millisecondi % 60000);
            //codice di errore
            //_error = _axisA + _axisB + _axisC + _axisD + _buttons;
        }

        //stringa da inviare a Simulink.
        //come id si prendere il carattere J
        //Formato: ID type(S o B) lunghezza-stringa axisA axisB axisC axisD buttons timestamps errore
        public override string ToString()
        {
            string message = String.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8:00000} {9} {10}", ASCIIHEADER, LENPLACEHOLDER, TYPE, _axisA, _axisB, _axisC, _axisD, _buttons, _time, ERRORPLACEHOLDER, ASCIIFOOTER);
            int len = message.Length;
            if (len < 100)
            {
                len--;
            }
            else if (len > 999)
            {
                len++;
            }
            message = message.Replace(LENPLACEHOLDER, len+"");
            int error = len + TYPE + _axisA + _axisB + _axisC + _axisD + _buttons + _time;
            message = message.Replace(ERRORPLACEHOLDER, String.Format("{0:00000}",Convert.ToUInt16(error & 0xFFFF)));
            return message;
        }


        //funzione che calcola il valore intero che rappresenta i bottoni premuti
        //Ho considerato l' array di bool come un numero binario e l ho trasformato in decimale
        //Alla ricezione di questo pacchetto, per sapere quali bottoni sono stati premuti
        //basterà riconvertire il numero in binario.
        private int CalcolaInteroButtons(bool[] _buttons)
        {
            int sum = 0;
            for (int i = 0; i < _buttons.Length; i++)
            {
                if (_buttons[i])
                    sum += Convert.ToInt32(Math.Pow(2, i));
            }
            return sum;
        }

        // FORMATO PACCHETTO BINARIO: 
   // FF FF FF F2 lunghezza(16 bit) 00 01 assi(16 bit) bottoni(16 bit) timestamp crc FF FF
        public byte[] ToBinary()
        {
            byte[] message = new byte[24];
            //HEADER
            message[0] = 0xFF;
            message[1] = 0xFF;
            message[2] = 0xFF;
            message[3] = 0xF2; // tutti i dati seguenti devono essere a 16 bit
            //fine header

            // preparo il posto nell' header x la lunghezza
            message[4] = 0x00;
            message[5] = 0x00;

            // type 00 01 per joystick
            message[6] = 0x00;
            message[7] = 0x01;

            // INIZIO DATI
            // ASSI
            message = Binary.Write2BValue(_axisA, 8, message);
            message = Binary.Write2BValue(_axisB, 10, message);
            message = Binary.Write2BValue(_axisC, 12, message);
            message = Binary.Write2BValue(_axisD, 14, message);
            //PULSANTI
            message = Binary.Write2BValue(_buttons, 16, message);
            //TIMESTAMP
            message = Binary.Write2BValue(_time, 18, message);
          
            //CODICE ERRORE CRC
            Crc16 crc = new Crc16();
            byte[] crc16 = crc.ComputeChecksumBytes(message);

            message[20] = crc16[0];
            message[21] = crc16[1];

            //FOOTER
            message[22] = 0xFF;
            message[23] = 0xFF;

            // LUNGHEZZA DA INSERIRE SUBITO DOPO L' HEADER.
            message = Binary.Write2BValue(message.Length, 4, message);

            return message;
        }
    }
}


